package clubhouse.motioncommand;

/**
 * @author Anis Darwich & Lionel Croix - Simulation and experimentation on Robot in WSN - WnSN Lab - SJTU
 */
public class MotionCommand {
	/**The value of default API type for Xbee message**/
	public static final int DEFAULT_API_TYPE = 16;

	/**Xbee message start delimiter.*/
	private Integer start_delimiter;
	/**API type of current message.*/
	private Integer api_type;

	/**Current value of distance info for the next move*/
	private Integer distance;
	/**Current value of rotation info for the next move*/
	private Integer rotation;
	/**Current value of rotation direction for the next move, 1 if trigonometric direction, 0 otherwise.*/
	private Integer trigo;

	/**Xbee message end delimiter.*/
	private Integer end_delimiter;

	/**
	 * Constructs the MotionCommand object directly from the specified move information.
	 * @param type the value of the Xbee message API type.
	 * @param a the number of pulses for distance move.
	 * @param b the number of pulses for rotation move.
	 * @param c the direction of rotation move, 1 if trigonometric direction, 0 otherwise.
	 */
	public MotionCommand (Integer type, Integer a, Integer b, Integer c) {
		this.start_delimiter = 255;
		
		this.api_type = type; // 16 or 17
		
		this.distance = a;
		this.rotation = b;
		this.trigo = c;
		
		this.end_delimiter = 254;
	}
	
	/**
	 * Constructs a generic NodeControl object (without pulses information).
	 */
	public MotionCommand () {
		this.start_delimiter = 255;
		this.end_delimiter = 254;
	}
	
	/**
	 * Return Api type field in message.
	 * @return the value of the API type.
	 */
	public Integer getApiType() {
		return this.api_type;
	}
	
	/**
	 * Return distance pulse field in message.
	 * @return the value of the distance pulse field.
	 */
	public Integer getDistancePulse() {
		return this.distance;
	}
	
	/**
	 * Return rotation pulse field in message.
	 * @return the value of the rotation pulse field.
	 */
	public Integer getRotationPulse() {
		return this.rotation;
	}
	
	/**
	 * Return rotation direction field in message.
	 * @return the value of the rotation direction field, 1 if trigonometric direction, 0 otherwise.
	 */
	public Integer getRotationDirection() {
		return this.trigo;
	}
	
	/**
	 * Convert from pulses to coordinates increment.
	 * @return the double array containing increment information (X coordinate, Y coordinate, direction in rad).
	 */
	public double[] getCoordIncrement() {
		if (this.trigo == 0) this.rotation= -rotation;
		return new double[] {
			(distance/CalculInfo.delta)*Math.cos(rotation/CalculInfo.omega),
			(distance/CalculInfo.delta)*Math.sin(rotation/CalculInfo.omega),
			rotation/CalculInfo.omega};
	}
	
	/**
	 * Return a string representation of the message written in Hexadecimal format.
	 * @return the string representing the whole message written in Hexadecimal format.
	 */
	public String toHexPrint(){
		return (Integer.toHexString(start_delimiter)+":" + Integer.toHexString(api_type)+":" + Integer.toHexString(distance)+":" + Integer.toHexString(rotation)+":" + Integer.toHexString(trigo)+":" + Integer.toHexString(end_delimiter));
	}
	
	/**
	 * Convert the message to a table of bytes in order to send it on the UART interface.
	 * @return the byte array to send to the UART interface.
	 */
	public byte[] toSend() {
		byte[] send_buffer = new byte[7];
		send_buffer[0] = this.start_delimiter.byteValue();
		send_buffer[1] = this.api_type.byteValue();
		send_buffer[2] = (byte)( (this.distance << 16) >> 24 );
		send_buffer[3] = (byte)( (this.distance << 24) >> 24 );
		send_buffer[4] = this.rotation.byteValue();
		send_buffer[5] = this.trigo.byteValue();
		send_buffer[6] = this.end_delimiter.byteValue();
		return send_buffer;
	}
	
	/**
	 * Parse the given string to a MotionCommand type.
	 * @param packet the message packet received to parse.
	 * @return the MotionCommand object created from the received packet.
	 */
	public static MotionCommand parse(String packet) {
		MotionCommand received = new MotionCommand();
		received.api_type = Integer.parseInt(packet.substring(2, 4), 16);
		received.distance = Integer.parseInt(packet.substring(4, 8), 16);
		received.rotation = Integer. parseInt(packet.substring(8, 10), 16);
		received.trigo = Integer. parseInt(packet.substring(10, 12), 16);
		
		//Uncomment the following lines to display received command packet in console.
		/*
		System.out.print("length " + packet.length()+ "\n"); //
		System.out.print("@ index 0-2: " + packet.substring(0, 2) + "\n"); // ff start delimiter
		System.out.print("@ index 2-4: " + packet.substring(2, 4) + "\n"); // 10 ou 11 api type
		System.out.print("@ index 4-8: " + packet.substring(4, 8) + "\n"); // 10 distance
		System.out.print("@ index 8-10: " + packet.substring(8, 10) + "\n"); // 3 rotation
		System.out.print("@ index 10-12: " + packet.substring(10, 12) + "\n"); // 0 ou 1 rotation direction
		*/
		
		return received;
	}
}	
	